{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Interactive Demo for Metrics\n",
    "\n",
    "* command line executables: see README.md\n",
    "* algorithm documentation: [metrics.py API & Algorithm Documentation](metrics.py_API_Documentation.ipynb)\n",
    "* **make sure you enabled interactive widgets via: **\n",
    "```\n",
    "sudo jupyter nbextension enable --py --sys-prefix widgetsnbextension\n",
    "```\n",
    "\n",
    "* **make sure you use the correct Kernel** matching the your evo Python version (otherwise use the menu Kernel->Change.."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "...some modules and settings for this demo:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "from __future__ import print_function\n",
    "\n",
    "from evo.tools import log\n",
    "log.configure_logging()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from evo.tools import plot\n",
    "from evo.tools.plot import PlotMode\n",
    "from evo.core.metrics import PoseRelation, Unit\n",
    "from evo.tools.settings import SETTINGS\n",
    "\n",
    "# temporarily override some package settings\n",
    "SETTINGS.plot_figsize = [6, 6]\n",
    "SETTINGS.plot_split = True\n",
    "SETTINGS.plot_usetex = False\n",
    "\n",
    "# magic plot configuration\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "%matplotlib notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# interactive widgets configuration\n",
    "import ipywidgets\n",
    "\n",
    "check_opts_ape = {\"align\": False, \"correct_scale\": False, \"show_plot\": True}\n",
    "check_boxes_ape=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_ape.items()]\n",
    "check_opts_rpe = {\"align\": False, \"correct_scale\": False, \"all_pairs\": False, \"show_plot\": True}\n",
    "check_boxes_rpe=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_rpe.items()]\n",
    "delta_input = ipywidgets.FloatText(value=1.0, description='delta', disabled=False, color='black')\n",
    "du_selector=ipywidgets.Dropdown(\n",
    "    options={u.value: u for u in Unit},\n",
    "    value=Unit.frames, description='delta_unit'\n",
    ")\n",
    "pm_selector=ipywidgets.Dropdown(\n",
    "    options={p.value: p for p in PlotMode},\n",
    "    value=PlotMode.xy, description='plot_mode'\n",
    ")\n",
    "pr_selector=ipywidgets.Dropdown(\n",
    "    options={p.value: p for p in PoseRelation},\n",
    "    value=PoseRelation.translation_part, description='pose_relation'\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "\n",
    "## Load trajectories"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from evo.tools import file_interface\n",
    "from evo.core import sync"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Load KITTI files** with entries of the first three rows of $\\mathrm{SE}(3)$ matrices per line (no timestamps):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_ref = file_interface.read_kitti_poses_file(\"../test/data/KITTI_00_gt.txt\")\n",
    "traj_est = file_interface.read_kitti_poses_file(\"../test/data/KITTI_00_ORB.txt\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**...or load a ROS bagfile** with `geometry_msgs/PoseStamped` topics:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import rosbag\n",
    "    bag_handle = rosbag.Bag(\"../test/data/ROS_example.bag\")\n",
    "    traj_ref = file_interface.read_bag_trajectory(bag_handle, \"groundtruth\")\n",
    "    traj_est = file_interface.read_bag_trajectory(bag_handle, \"ORB-SLAM\")\n",
    "    traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)\n",
    "except ImportError as e:\n",
    "    print(e)  # ROS not found"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**... or load TUM files with** 3D position and orientation quaternion per line ($x$ $y$ $z$ $q_x$ $q_y$ $q_z$ $q_w$):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_ref = file_interface.read_tum_trajectory_file(\"../test/data/fr2_desk_groundtruth.txt\")\n",
    "traj_est = file_interface.read_tum_trajectory_file(\"../test/data/fr2_desk_ORB_kf_mono.txt\")\n",
    "traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(traj_ref)\n",
    "print(traj_est)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "\n",
    "## APE\n",
    "\n",
    "Algorithm and API explanation: [see here](metrics.py_API_Documentation.ipynb#ape_math)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Interactive APE Demo\n",
    "***Run the code below, configure the parameters in the GUI and press the update button.***\n",
    "\n",
    "(uses the trajectories loaded above)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "import evo.main_ape as main_ape\n",
    "import evo.common_ape_rpe as common\n",
    "\n",
    "count = 0\n",
    "results = []\n",
    "\n",
    "def callback_ape(pose_relation, align, correct_scale, plot_mode, show_plot):\n",
    "    global results, count\n",
    "    est_name=\"APE Test #{}\".format(count)\n",
    "    \n",
    "    result = main_ape.ape(traj_ref, traj_est, est_name=est_name,\n",
    "                          pose_relation=pose_relation, align=align, correct_scale=correct_scale)\n",
    "    count += 1\n",
    "    results.append(result)\n",
    "    \n",
    "    if show_plot:\n",
    "        fig = plt.figure()\n",
    "        ax = plot.prepare_axis(fig, plot_mode)\n",
    "        plot.traj(ax, plot_mode, traj_ref, style=\"--\", alpha=0.5)\n",
    "        plot.traj_colormap(\n",
    "            ax, result.trajectories[est_name], result.np_arrays[\"error_array\"], plot_mode,\n",
    "            min_map=result.stats[\"min\"], max_map=result.stats[\"max\"])\n",
    "    \n",
    "_ = ipywidgets.interact_manual(callback_ape, pose_relation=pr_selector, plot_mode=pm_selector,\n",
    "                               **{c.description: c.value for c in check_boxes_ape})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "\n",
    "## RPE\n",
    "\n",
    "Algorithm and API explanation: [see here](metrics.py_API_Documentation.ipynb#rpe_math)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Interactive RPE Demo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "***Run the code below, configure the parameters in the GUI and press the update button.***\n",
    "\n",
    "(uses the trajectories loaded above, alignment only useful for visualization here)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "import evo.main_rpe as main_rpe\n",
    "\n",
    "count = 0\n",
    "results = []\n",
    "\n",
    "def callback_rpe(pose_relation, delta, delta_unit, all_pairs, align, correct_scale, plot_mode, show_plot):\n",
    "    global results, count\n",
    "    est_name=\"RPE Test #{}\".format(count)\n",
    "    result = main_rpe.rpe(traj_ref, traj_est, est_name=est_name,\n",
    "                          pose_relation=pose_relation, delta=delta, delta_unit=delta_unit, \n",
    "                          all_pairs=all_pairs, align=align, correct_scale=correct_scale, \n",
    "                          support_loop=True)\n",
    "    count += 1\n",
    "    results.append(result)\n",
    "    \n",
    "    if show_plot:\n",
    "        fig = plt.figure()\n",
    "        ax = plot.prepare_axis(fig, plot_mode)\n",
    "        plot.traj(ax, plot_mode, traj_ref, style=\"--\", alpha=0.5)\n",
    "        plot.traj_colormap(\n",
    "            ax, result.trajectories[est_name], result.np_arrays[\"error_array\"], plot_mode,\n",
    "            min_map=result.stats[\"min\"], max_map=result.stats[\"max\"])\n",
    "\n",
    "_ = ipywidgets.interact_manual(callback_rpe, pose_relation=pr_selector, plot_mode=pm_selector, \n",
    "                               delta=delta_input, delta_unit=du_selector, \n",
    "                               **{c.description: c.value for c in check_boxes_rpe})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Do stuff with the result objects:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "from evo.tools import pandas_bridge\n",
    "\n",
    "df = pd.DataFrame()\n",
    "for result in results:\n",
    "    df = pd.concat((df, pandas_bridge.result_to_df(result)), axis=\"columns\")\n",
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "df.loc[\"stats\"]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.15rc1"
  },
  "widgets": {
   "state": {
    "54cc6cb1d20f45438fb3663d98d29406": {
     "views": [
      {
       "cell_index": 21
      }
     ]
    },
    "88c180f9f59147a592d738936cecf614": {
     "views": [
      {
       "cell_index": 16
      }
     ]
    }
   },
   "version": "1.2.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
